本发明属于车辆定位技术领域,特别涉及一种基于GPS和虚拟卫星组合定位检查列车完整性的方法。当列车尾部的GPS接收机只能接收到3颗可见卫星信号时,利用列车头部机车上方的GPS接收机输出的经度、纬度和高程信息为列车尾部定位提供一颗假想卫星,即虚拟卫星,并建立第四个约束方程,实现列车尾部的定位解算,最后把列车头部和列车尾部坐标投影到高斯平面,实时计算列车长度,完成列车完整性检查。利用本发明在列车尾部GPS接收机接收卫星信号不完备条件下,仍然可以进行列车完整性检查,保证了列车的行车安全。
1.一种基于GPS和虚拟卫星组合定位检查列车完整性的方法,列车将2个 GPS接收机分别置于列车头部的机车上方和列车尾部的车钩上面,其特征在于: 当列车尾部的GPS接收机只能接收到3颗可见卫星信号时,利用列车头部机车 上方的GPS接收机输出的经度、纬度和高程信息为列车尾部定位提供一颗虚拟 卫星,建立第四个约束方程,并联立列车尾部GPS接收机接收的3颗可见卫星 信号得到的三个约束方程,从而得出列车尾部三维坐标,实现列车尾部定位,具 体包括如下步骤: 步骤一,为建立第四个约束方程,在列车头部的机车上方P′点沿椭球法线的 延长线与Z轴的交点处建立一颗虚拟卫星,该虚拟卫星的三维坐标为(0,0,z_c), 虚拟卫星到列车尾部的伪距ρ₄用它到列车头部的机车上方的几何距离S来表示, 即ρ₄=S;其中:S=N_P+H_P,N_p为列车头部的卯酉圈曲率半径,H_p为列车头部 的高程,则第四个约束方程为 ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 , ]]>其中:x₄=0,y₄=0,z₄=z_c为虚拟卫星的三维坐标,(x_u,y_u,z_u)为要求解的列车尾部 坐标; 步骤二, 建立观测方程,求解列车尾部坐标,列车尾部GPS接收机接收到的三颗可 见卫星信号得到三个约束方程为: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ]]>
......................................3式 ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ]]>
......................................4式 ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ]]>
......................................5式 其中(x_j,y_j,z_j)和ρ_j分别是可见卫星的三维坐标及可见卫星到列车尾部的 伪距,j=1,2,3;δt_u为接收机时钟与系统时钟之间的偏移; 联立上述四个方程,得到如下非线性方程组: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 ]]>6式 在上述第四个约束方程ρ_j=ρ₄,j=4; 对上述非线性方程组进行线性化处理 假设ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="73"/>和ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="20"/>分别为(x_u,y_u,z_u)和δt_u的估计值,(Δx_u,Δy_u,Δz_u)和Δt_u分别为 (x_u,y_u,z_u)和δt_u估计值的偏移量,令: ρ j = ( x j - x u ) 2 + ( y j - y u ) 2 + ( z j - z u ) 2 + cδ t u = f j ( x u , y u , z u , δ t u ) ]]>
.....................7式 ρ ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 + cδ t ^ u = f j ( x ^ u , y ^ u , z ^ u , δ t ^ u ) ]]>
.....................8式 即有: f j ( x u , y u , z u , δ t u ) = f j ( x ^ u + Δ x u , y ^ u + Δ y u , z ^ u + Δ z u , δ t ^ u + Δ t u ) ]]>
.....................9式 将9式等号右边的关系式用泰勒级数在点ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="102"/>展开并忽略高次项,并 将7式、8式代入基本方程9式,经整理得到线性关系式: ρ j = ρ ^ j - x j - x ^ u r ^ j Δ x u - y j - y ^ u r ^ j Δ y u - z j - z ^ u r ^ j Δ z u + cΔ t u ]]>
............................10式 其中 r ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 ]]>引入下述新变量以简化10式: Δ ρ j = ρ ^ j - ρ j a xj = x j - x ^ u r ^ j a yj = y j - y ^ u r ^ j a zj = z j - z ^ u r ^ j ]]>
.....................................................11式 在上述7-11式中,当Δt_u、δt_u为0时,ρ_j=ρ₄,j=4; 通过上述的线性化得到如下方程组: Δ ρ 1 = a x 1 Δ x u + a y 1 Δ y u + a z 1 Δ z u - cΔ t u Δ ρ 2 = a x 2 Δ x u + a y 2 Δ y u + a z 2 Δ z u - cΔ t u Δ ρ 3 = a x 3 Δ x u + a y 3 Δ y u + a z 3 Δ z u - cΔ t u Δ ρ 4 = a x 4 Δ x u + a y 4 Δ y u + a z 4 Δ z u ]]>
.....................................................12式 写成矩阵形式:Δρ=HΔx.........................13式 其中: Δρ = Δ ρ 1 Δ ρ 2 Δ ρ 3 Δ ρ 4 , ]]> H = a x 1 a y 1 a z 1 1 a x 2 a y 2 a z 2 1 a x 3 a y 3 a z 3 1 a x 4 a y 4 a z 4 0 , ]]> ΔX = Δ x u Δ y u Δ z u - cΔ t u ]]>其解为:ΔX=H^-1Δρ 则列车尾部位置坐标为 x u = x ^ u + Δ x u y u = y ^ u + Δ y u z u = z ^ u + Δ z u , ]]>且 δ t u = δ t ^ u + Δ t u ]]>步骤三,计算列车长度,判断列车是否完整, 根据以上算法可以计算出列车尾部的三维坐标,然后把列车头部坐标和列车 尾部坐标投影到高斯平面,实时计算出列车长度 L = ( x 1 - x 2 ) 2 + ( y 1 - y 2 ) 2 , ]]>其中: (x₁,y₁)为列车头部平面坐标,(x₂,y₂)为列车尾部平面坐标;通过与列车实际长度相 比,则实现列车实时完整性检查。 技术领域 本发明属于车辆定位技术领域,特别涉及一种基于GPS和虚拟卫星组合定 位检查列车完整性的方法。 背景技术 由于列车车厢通过挂钩相互连接而成,为保证列车不脱轨、不脱钩,确保列 车的完整而进行的检查称为列车完整性检查。列车完整性检查是关系到铁路系统 安全性的一个关键环节,如果列车完整性检查失效,遗留在区间或者股道上的车 辆将成为后续列车的障碍物,直接危及行车安全。 目前,常用的列车完整性检查方法一种是列车尾部安装安全防护装置(简称 列车尾部装置)。列车尾部装置通过列车无线调度系统中的机车电台与本列车的 机车车载装置进行通信。机车电台与列车尾部装置主机通过无线编码构成“一对 一”关系,一旦此信号中断,则可以认为列车完整性出现问题,但该方法面临着 管理、调度、维护、责任者等多方面问题。另一种方法是列车尾部装置能实时检 查列车尾部风管风压,并将风压信息不停的反馈到机车车载装置,实现欠压报警, 提示司机采取紧急制动等应急措施,可以有效防止列车在运行中折角塞门关闭、 风管折断、风管漏风或列车分离等事故发生。当列车分离,风管断开漏风,泄漏 量超过规定值时,列车尾部装置会及时向机车乘务员发出警示,提醒机车乘务员 注意列车运行状态。但列车尾部装置在使用过程中还存在一些问题,如:列车尾 部装置在某些长大隧道或隧道群等弱场强区段,无线列调所具备的中继通信功能 不能正常使用;既有或新增的无线列调,没有列车尾部装置司机控制盒的预留接 口,给安装和使用带来困难;无线列调的使用频率不当,造成枢纽内列车尾部装 置主机与无线列调间相互干扰,影响列车的出发;列车尾部装置对风压的查询频 率不够,有的是一分钟甚至几分钟查询一次,这样完整性检查的实时性太差。 全球定位系统(Globalxa0Positioningxa0System)即GPS,它是利用空间卫星作 为导航台完成无线电定位的系统。每一颗卫星连续不断的向GPS接收机发送可跟 踪的唯一编码序列,GPS接收机可根据编码辨认相关卫星,进而计算出接收机的 确切位置和准确时间,其中伪距法是GPS导航定位的基本方法。 近年来,卫星定位技术应用得到飞速的发展,卫星定位广泛应用在航空、空 间运载引导、航海和陆地,其中陆地的应用研究主要在城市交通、特种车辆如警 车、运钞车管理调度等领域,而在铁路应用中较少研究。 如何将卫星定位技术引入到列车完整性检查还没有成熟地理论与技术,目前 在实际应用中不可能完全取代列车尾部装置来独立进行列车完整性检查,但是可 以辅助或者结合目前的列车尾部装置,通过与列车尾部装置信息比较与融合,来 构成一个冗余的列车完整性检查系统。青藏铁路对于列车完整性检查是用列车尾 部装置实现的,并可以在列车运行的全过程结合轨旁装置或通过车载设备对列车 的完整性实施不间断检查。基于卫星定位的列车完整性检查系统是通过对列车头 部和列车尾部实时定位测算车长实现的,在列车行驶的过程中如果列车尾部GPS 接收机的可见空中范围受阻(如车厢遮挡),列车尾部接收到的可见卫星数目不 足四颗,常规的卫星定位方法受到限制。 发明内容 现有技术中安装在列车头部和列车尾部的GPS接收机对列车头部和列车尾部 实时定位,然后根据位置信息实时地测算车长实现的,当发现列车有抛车现象时, 立即发出报警信息。但在实际运行情况中,由于列车尾部车厢的遮挡,列车尾部 GPS接收机仅能接收到三颗可见卫星信号,因此常规的卫星定位方法受到了限制, 本发明特别提出了一种基于GPS和虚拟卫星组合定位检查列车完整性的方法。 本发明的技术方案如下: 列车在行驶的过程中,安装在列车头部和列车尾部的GPS接收机不间断的接 收卫星信号进行定位解算,实时输出列车头部和列车尾部的经度、纬度和高程信 息。若某时刻列车尾部GPS接收机由于车厢的遮挡只接收到了三颗可见卫星的信 号,根据GPS卫星定位的基本原理,此时列车尾部的接收机没有接收到足够的卫 星数目,不能实时输出列车尾部的位置信息。此时利用列车头部GPS接收机输出 的经度、纬度和高程信息为列车尾部定位提供一颗假想卫星,即虚拟卫星,并建 立第四个约束方程,可实现列车尾部的定位解算。最后把列车头部和列车尾部坐 标投影到高斯平面,实时计算列车长度,完成列车完整性检查。 根据我国铁路线路标准规定,在III级铁路的困难地段,轨道坡度的限制为千 分之二十,按300米列车长度计算,列车头部和列车尾部高差也仅为0.9米,可 以近似的认为高程也是相差0.9米。同时,按列车南北方向行驶,此时纬度相差 最大,为α=D/r,其中D为列车长度,r为地球平均半径(6371.03km)。按300 米列车长度计算,纬度相差4.71×10^-5弧度。此时的卯酉圈曲率半径 N P = a 1 - ( e sin B P ) 2 ]]>仅相差4.73×10^-5米。因此通过利用列车头部信息确定虚拟卫星, 把列车头部到虚拟卫星的几何距离S当作列车尾部到虚拟卫星的距离ρ₄是合理 可用的。 列车实时完整性检查的具体实现步骤如下: 步骤一,建立第四个约束方程, 在列车头部的机车上方P′点沿椭球法线的延长线与Z轴的交点处建立一颗 虚拟卫星,该卫星的三维坐标为(0,0,z_c),虚拟卫星到列车尾部的伪距ρ₄用 它到列车头部的机车上方的几何距离S来表示,即ρ₄=S;其中:S=N_P+H_P,N_p为列车头部的卯酉圈曲率半径,H_p为列车头部的高程,则第四个约束方程为 ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 , ]]>其中:x₄=0,y₄=0,z₄=z_c为虚拟卫星的三维坐标,(x_u,y_u,z_u)为要求解的列车尾 部坐标; 步骤二,建立观测方程,求解列车尾部坐标, 列车尾部接收机接收到的三颗可见卫星信号得到三个约束方程为: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ]]>.............................................3式 ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ]]>.......................................4式 ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ]]>............................................5式 其中(x_i,y_i,z_j)和ρ_j分别是可见卫星的三维坐标及可见卫星到列车尾部的 伪距,j=1,2,3;δt_u为GPS接收机时钟与系统时钟之间的偏移; 联立上述四个方程,得到如下非线性方程组: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 ]]>6式 在上述第四个约束方程ρ_j=ρ₄,j=4; 对上述非线性方程组进行线性化处理 假设ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="72"/>和ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="20"/>分别为(x_u,y_u,z_u)和δt_u的估计值,(Δx_u,Δy_u,Δz_u)和Δt_u分别为 (x_u,y_u,z_u)和δt_u估计值的偏移量,令: ρ j = ( x j - x u ) 2 + ( y j - y u ) 2 + ( z j - z u ) 2 + cδ t u = f j ( x u , y u , z u , δt u ) ]]>........................7式 ρ ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 + cδ t ^ u = f j ( x ^ u , y ^ u , z ^ u , δ t ^ u ) ]]>........................8式 即有: f j ( x u , y u , z u , δ t u ) = f j ( x ^ u + Δ x u , y ^ u + Δ y u , z ^ u + Δ z u , δ t ^ u + Δ t u ) ]]>..............................9式 将9式等号右边的关系式用泰勒级数在点ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="102"/>展开并忽略高次项,并 将7式、8式代入基本方程9式,经整理得到线性关系式: ρ j = ρ ^ j - x j - x ^ u r ^ j Δ x u - y j - y ^ u r ^ j Δ y u - z j - z ^ u r ^ j Δ z u + cΔ t u ]]>....................................10式 其中 r ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 ]]>引入下述新变量以简化10式: Δ ρ j = ρ ^ j - ρ j a xj = x j - x ^ u r ^ j a yj = y j - y ^ u r ^ j a zj = z j - z ^ u r ^ j ]]>.....................................................................11式 通过上述的线性化得到如下方程组: Δ ρ 1 = a x 1 Δ x u + a y 1 Δ y u + a z 1 Δ z u - cΔ t u Δ ρ 2 = a x 2 Δ x u + a y 2 Δ y u + a z 2 Δ z u - cΔ t u Δ ρ 3 = a x 3 Δ x u + a y 3 Δ y u + a z 3 Δ z u - cΔ t u Δ ρ 4 = a x 4 Δ x u + a y 4 Δ y u + a z 4 Δ z u ]]>................................................12式 写成矩阵形式: Δρ=HΔx........................................................................13式 其中: Δρ = Δρ 1 Δρ 2 Δρ 3 Δρ 4 , ]]> H = a x 1 a y 1 a z 1 1 a x 2 a y 2 a z 2 1 a x 3 a y 3 a z 3 1 a x 4 a y 4 a z 4 0 , ]]>ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="111"/>其解为:ΔX=H^-1Δρ 则列车尾部位置坐标为 x u = x ^ u + Δ x u y u = y ^ u + Δ y u z u = z ^ u + Δ z u , ]]>且 δ t u = δ t ^ u + Δ t u ]]>步骤三,计算列车长度,判断列车是否完整,完成列车完整性检查 根据以上算法可以计算出列车尾部的三维坐标,然后把车头坐标和列车尾部 坐标投影到高斯平面,实时计算出列车长度 L = ( x 1 - x 2 ) 2 + ( y 1 - y 2 ) 2 . ]]>其中:(x₁,y₁) 为车头平面坐标,(x₂,y₂)为列车尾部平面坐标。通过与列车实际长度相比,则实 现列车实时完整性检查。 本发明的有益效果是:当列车尾部GPS接收机仅能接收到三颗可见卫星信号, 通过列车头部提供的纬度、经度和高程信息,增加了第四个约束方程仍然可以实 现定位,克服了现有技术中的不足,仍然可以进行列车完整性检查,保证了列车 的行车安全。 附图说明 图1是GPS不完备条件下列车完整性检查系统结构图。 图2是GPS和虚拟卫星组合定位检查列车完整性程序流程图。 图3是子午面直角坐标系下通过列车头部位置确定虚拟卫星位置示意图。 具体实施方式 下面通过附图和具体实施方式对本发明作进一步详细说明。 参见附图1,列车在行驶的过程中,通过列车信息传输系统把列车头部接收 机输出的经度、纬度和高程信息发送给列车尾部设备,在已知列车头部和列车尾 部三维坐标的情况下通过实时测算车长实现列车完整性检查,当发现列车有抛车 现象时,立即发出报警信息。但由于车厢的遮挡,列车尾部GPS接收机仅能接收 到三颗可见卫星信号,常规的卫星定位方法受到限制。 通过下面实施例,结合图2、图3对本发明做进一步说明。 实施例: 1、采集可视卫星的星历、伪距数据,接收车头信息(如图3所示),用GPS 接收机采集可视卫星的伪距、星历数据,通过列车信息传输系统接收列车头部接 收机输出的经度、纬度和高程信息。 2、计算可视卫星坐标; 3、对卫星坐标和伪距进行误差修正 本发明考虑到实用性,对卫星坐标和伪距误差进行了地球旋转效应误差修 正、卫星信号发射时刻误差修正和相对论误差修正。 4、通过车头信息建立虚拟卫星 图2所示,空间大地坐标依附于参考椭球。为建立大地坐标与直角坐标的数 学关系,必须首先定义参考椭球。建立子午面直角坐标系XOZ。因为参考椭球是 旋转对称的,所以只需在XOZ平面直角坐标系内作为二维问题考虑。 P′点为列车头部的位置; P点为P′点沿椭球法线量测到椭球面的交点; H_p为P′点沿椭球法线量测到椭球面的距离,称为椭球面高程,简称为大地高; W点为过P点的卯酉圈的曲率中心,位于椭球的旋转轴上,即P′点沿椭球法 线的延长线与Z轴的交点; N_p为过P点的卯酉圈曲率半径,等于法线介于椭球面和短轴之间的长度,即 P点到W点的距离。 在W点建立一颗虚拟卫星,并将当前列车头部经度、纬度和高程的大地坐标 转换成WGS-84空间直角坐标: x = ( N p + H p ) cos B · cos L y = ( N p + H p ) cos B · sin L z = [ N p ( 1 - e 2 ) + H p ] sin B ]]>...............................................................1式 由卯酉圈曲率半径公式 N P = a 1 - ( e sin B P ) 2 ]]>可求得虚拟卫星到列车头部的几何 距离为:S=N_P+H_P,其中a为地球参考椭球的长半轴,e为地球参考椭球偏心率。 由两点之间的距离公式 S = ( x - x 4 ) 2 + ( y - y 4 ) 2 + ( z - z 4 ) 2 ]]>求解虚拟卫星的三维坐 标,其中x₄=0,y₄=0,z₄=z_c为虚拟卫星的三维坐标。 5、建立第四个约束方程 由虚拟卫星得到求解列车尾部实时位置的第四个约束方程: ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 ]]>................................................2式 其中(x_u,y_u,z_u)为要求解的列车尾部坐标。 6、建立观测方程,求解列车尾部坐标 列车尾部接收机接收到三颗可见卫星的信息,得到三个约束方程为: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ]]>.............................................3式 ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ]]>.............................................4式 ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ]]>.............................................5式 其中x_j,y_j,,z_j,ρ_j,j=1,2,3是可见卫星的三维坐标及伪距,(x_u,y_u,z_u)为 列车尾部坐标,δt_u为接收机时钟与系统时之间的偏移。 联立上述四个方程,得到如下非线性方程组: ρ 1 = ( x 1 - x u ) 2 + ( y 1 - y u ) 2 + ( z 1 - z u ) 2 + cδ t u ρ 2 = ( x 2 - x u ) 2 + ( y 2 - y u ) 2 + ( z 2 - z u ) 2 + cδ t u ρ 3 = ( x 3 - x u ) 2 + ( y 3 - y u ) 2 + ( z 3 - z u ) 2 + cδ t u ρ 4 = ( x 4 - x u ) 2 + ( y 4 - y u ) 2 + ( z 4 - z u ) 2 ]]>.......................................6式 在上述第四个约束方程ρ_j=ρ₄,j=4; 对上述非线性方程组进行线性化处理 首先假设ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="73"/>和ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="20"/>分别为(x_u,y_u,z_u)和δt_u的估计值,(Δx_u,Δy_u,Δz_u)和Δt_u分别 为(x_u,y_u,z_u)和δt_u估计值的偏移量,令: ρ j = ( x j - x u ) 2 + ( y j - y u ) 2 + ( z j - z u ) 2 + cδ t u = f j ( x u , y u , z u , δt u ) ]]>........................7式 ρ ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 + cδ t ^ u = f j ( x ^ u , y ^ u , z ^ u , δ t ^ u ) ]]>........................8式 即有: f j ( x u , y u , z u , δ t u ) = f j ( x ^ u + Δ x u , y ^ u + Δ y u , z ^ u + Δ z u , δ t ^ u + Δ t u ) ]]>..............................9式 将9式等号右边的关系式用泰勒级数在点ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="102"/>展开并忽略高次项,并 将7式、8式代入基本方程经整理得到线性关系式: ρ j = ρ ^ j - x j - x ^ u r ^ j Δ x u - y j - y ^ u r ^ j Δ y u - z j - z ^ u r ^ j Δ z u + cΔ t u ]]>....................................10式 其中 r ^ j = ( x j - x ^ u ) 2 + ( y j - y ^ u ) 2 + ( z j - z ^ u ) 2 ]]>引入下述新变量以简化10式: Δ ρ j = ρ ^ j - ρ j a xj = x j - x ^ u r ^ j a yj = y j - y ^ u r ^ j a zj = z j - z ^ u r ^ j ]]>.....................................................................11式 通过上述的线性化得到如下方程组: Δ ρ 1 = a x 1 Δ x u + a y 1 Δ y u + a z 1 Δ z u - cΔ t u Δ ρ 2 = a x 2 Δ x u + a y 2 Δ y u + a z 2 Δ z u - cΔ t u Δ ρ 3 = a x 3 Δ x u + a y 3 Δ y u + a z 3 Δ z u - cΔ t u Δ ρ 4 = a x 4 Δ x u + a y 4 Δ y u + a z 4 Δ z u ]]>................................................12式 写成矩阵形式:Δρ=HΔx.............................................13式 其中: Δρ = Δρ 1 Δρ 2 Δρ 3 Δρ 4 , ]]> H = a x 1 a y 1 a z 1 1 a x 2 a y 2 a z 2 1 a x 3 a y 3 a z 3 1 a x 4 a y 4 a z 4 0 , ]]> ΔX = Δ x u Δ y u Δ z u - cΔ t u ]]>其解为:ΔX=H^-1Δρ 则列车尾部位置坐标为 x u = x ^ u + Δ x u y u = y ^ u + Δ y u z u = z ^ u + Δ z u , ]]>且 δ t u = δ t ^ u + Δ t u ]]>如果偏移超过了可接受的值,便重新迭代上述过程,即以算出的点坐标 (x_u,y_u,z_u)和δt_u作为新的估计值,以代替ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="33"/>7、计算列车长度,判断列车是否完整,完成列车完整性检查 根据以上算法可以计算出列车尾部的三维坐标,然后把车头坐标和列车尾部 坐标投影到高斯平面,实时计算出列车长度 L = ( x 1 - x 2 ) 2 + ( y 1 - y 2 ) 2 . ]]>其中:(x₁,y₁) 为车头平面坐标,(x₂,y₂)为列车尾部平面坐标(如图2所示)。